//
// Created by jg on 2021/3/2.
//

#include "active_location_path/active_location_path.h"

void AL_Path::init(bool _is_use_alpath) {
    //if is_use_alpath_ == false   find the shortest path
    is_use_alpath_ = _is_use_alpath;

    Eigen::Vector3d pos;
    /* add node 0 */
    pos << 51.701, 62.923, 0.01;
    gragh_.push_back(std::make_shared<alpath_graph::GraphNode>(pos, 0));

    /* add node 1 */
    pos << 52.186, 8.370, 0.01;
    gragh_.push_back(std::make_shared<alpath_graph::GraphNode>(pos, 1));
    /* edge 0 1 */
    createEdge(is_use_alpath_ ? 0.401 : 1.0, gragh_[0], gragh_[1]);

    /* add node 2 */
    pos << 53.941, -55.783, 0.01;
    gragh_.push_back(std::make_shared<alpath_graph::GraphNode>(pos, 2));
    /* edge 1 2 */
    createEdge(is_use_alpath_ ? 0.331 : 1.0, gragh_[1], gragh_[2]);

    /* add node 3 */
    pos << 0.126, 61.922, 0.01;
    gragh_.push_back(std::make_shared<alpath_graph::GraphNode>(pos, 3));
    /* edge 0 3 */
    createEdge(is_use_alpath_ ? 0.632 : 1.0, gragh_[0], gragh_[3]);

    /* add node 4 */
    pos << 2.288, 2.970, 0.01;
    gragh_.push_back(std::make_shared<alpath_graph::GraphNode>(pos, 4));
    /* edge 3 4 */
    createEdge(is_use_alpath_ ? 0.573 : 1.0, gragh_[3], gragh_[4]);

    /* add node 5 */
    pos << 1.133, -57.144, 0.01;
    gragh_.push_back(std::make_shared<alpath_graph::GraphNode>(pos, 5));
    /* edge 4 5 */
    createEdge(is_use_alpath_ ? 0.373 : 1.0, gragh_[4], gragh_[5]);
    /* edge 2 5 */
    createEdge(is_use_alpath_ ? 0.632 : 1.0, gragh_[2], gragh_[5]);

    /* add node 6 */
    pos << -36.724, 61.787, 0.01;
    gragh_.push_back(std::make_shared<alpath_graph::GraphNode>(pos, 6));
    /* edge 3 6 */
    createEdge(is_use_alpath_ ? -0.243 : 1.0, gragh_[3], gragh_[6]);

    /* add node 7 */
    pos << -38.933, 2.613, 0.01;
    gragh_.push_back(std::make_shared<alpath_graph::GraphNode>(pos, 7));
    /* edge 6 7 */
    createEdge(is_use_alpath_ ? -0.317 : 1.0, gragh_[6], gragh_[7]);
    /* edge 4 7 */
    createEdge(is_use_alpath_ ? 0.043 : 1.0, gragh_[4], gragh_[7]);

    /* add node 8 */
    pos << -39.486, -58.049, 0.01;
    gragh_.push_back(std::make_shared<alpath_graph::GraphNode>(pos, 8));
    /* edge 7 8 */
    createEdge(is_use_alpath_ ? -0.321 : 1.0, gragh_[7], gragh_[8]);
    /* edge 5 8 */
    createEdge(is_use_alpath_ ? -0.076 : 1.0, gragh_[5], gragh_[8]);

    /* add node 9 */
    pos << -56.094, 2.559, 0.01;
    gragh_.push_back(std::make_shared<alpath_graph::GraphNode>(pos, 9));
    /* edge 7 9 */
    createEdge(is_use_alpath_ ? -0.308 : 1.0, gragh_[7], gragh_[9]);

    /* add node 10 */
    pos << -65.378, -58.011, 0.01;
    gragh_.push_back(std::make_shared<alpath_graph::GraphNode>(pos, 10));
    /* edge 8 10 */
    createEdge(is_use_alpath_ ? -0.308 : 1.0, gragh_[8], gragh_[10]);

    /* 新增节点 -图书馆到1号教学楼之间（暂无定位数据，权值赋为0.0） */
    /* add node 11 */
    pos << 50.856, -78.710, 0.01;
    gragh_.push_back(std::make_shared<alpath_graph::GraphNode>(pos, 11));
    /* edge 2 11 */
    createEdge(is_use_alpath_ ? 0.1 : 1.0, gragh_[2], gragh_[11]);

    /* add node 12 */
    pos << 112.549, 61.657, 0.01;
    gragh_.push_back(std::make_shared<alpath_graph::GraphNode>(pos, 12));
    /* edge 0 12 */
    createEdge(is_use_alpath_ ? 0.1 : 1.0, gragh_[0], gragh_[12]);

    /* add node 13 */
    pos << 109.983, -57.802, 0.01;
    gragh_.push_back(std::make_shared<alpath_graph::GraphNode>(pos, 13));
    /* edge 2 13 */
    createEdge(is_use_alpath_ ? 0.1 : 1.0, gragh_[2], gragh_[13]);

    /* add node 14 */
    pos << 197.155, 63.234, 0.01;
    gragh_.push_back(std::make_shared<alpath_graph::GraphNode>(pos, 14));
    /* edge 12 14 */
    createEdge(is_use_alpath_ ? 0.1 : 1.0, gragh_[12], gragh_[14]);

    /* add node 15 */
    pos << 205.513, -57.595, 0.01;
    gragh_.push_back(std::make_shared<alpath_graph::GraphNode>(pos, 15));
    /* edge 13 15 */
    createEdge(is_use_alpath_ ? 0.1 : 1.0, gragh_[13], gragh_[15]);

    /* add node 16 */
    pos << 254.932, 63.526, 0.01;
    gragh_.push_back(std::make_shared<alpath_graph::GraphNode>(pos, 16));
    /* edge 14 16 */
    createEdge(is_use_alpath_ ? 0.1 : 1.0, gragh_[14], gragh_[16]);

    /* add node 17 */
    pos << 255.615, 2.808, 0.01;
    gragh_.push_back(std::make_shared<alpath_graph::GraphNode>(pos, 17));
    /* edge 16 17 */
    createEdge(is_use_alpath_ ? 0.1 : 1.0, gragh_[16], gragh_[17]);

    /* add node 18 */
    pos << 256.415, -57.470, 0.01;
    gragh_.push_back(std::make_shared<alpath_graph::GraphNode>(pos, 18));
    /* edge 17 18 */
    createEdge(is_use_alpath_ ? 0.1 : 1.0, gragh_[17], gragh_[18]);
    /* edge 15 18 */
    createEdge(is_use_alpath_ ? 0.1 : 1.0, gragh_[15], gragh_[18]);

    /* add node 19 */
    pos << 287.376, 62.955, 0.01;
    gragh_.push_back(std::make_shared<alpath_graph::GraphNode>(pos, 19));
    /* edge 16 19 */
    createEdge(is_use_alpath_ ? 0.1 : 1.0, gragh_[16], gragh_[19]);

    /* add node 20 */
    pos << 289.482, 1.444, 0.01;
    gragh_.push_back(std::make_shared<alpath_graph::GraphNode>(pos, 20));
    /* edge 19 20 */
    createEdge(is_use_alpath_ ? 0.1 : 1.0, gragh_[19], gragh_[20]);
    /* edge 17 20 */
    createEdge(is_use_alpath_ ? 0.1 : 1.0, gragh_[17], gragh_[20]);

    /* add node 21 */
    pos << 290.294, -57.952, 0.01;
    gragh_.push_back(std::make_shared<alpath_graph::GraphNode>(pos, 21));
    /* edge 20 21 */
    createEdge(is_use_alpath_ ? 0.1 : 1.0, gragh_[20], gragh_[21]);
    /* edge 18 21 */
    createEdge(is_use_alpath_ ? 0.1 : 1.0, gragh_[18], gragh_[21]);

    /* add node 22 */
    pos << 318.504, 63.065, 0.01;
    gragh_.push_back(std::make_shared<alpath_graph::GraphNode>(pos, 22));
    /* edge 19 22 */
    createEdge(is_use_alpath_ ? 0.1 : 1.0, gragh_[19], gragh_[22]);

    /* add node 23 */
    pos << 318.417, 25.868, 0.01;
    gragh_.push_back(std::make_shared<alpath_graph::GraphNode>(pos, 23));
    /* edge 22 23 */
    createEdge(is_use_alpath_ ? 0.1 : 1.0, gragh_[22], gragh_[23]);

    /* add node 24 */
    pos << 318.836, -22.373, 0.01;
    gragh_.push_back(std::make_shared<alpath_graph::GraphNode>(pos, 24));
    /* edge 23 24 */
    createEdge(is_use_alpath_ ? 0.1 : 1.0, gragh_[23], gragh_[24]);

    /* add node 25 */
    pos << 318.887, -57.585, 0.01;
    gragh_.push_back(std::make_shared<alpath_graph::GraphNode>(pos, 25));
    /* edge 24 25 */
    createEdge(is_use_alpath_ ? 0.1 : 1.0, gragh_[24], gragh_[25]);
    /* edge 21 25 */
    createEdge(is_use_alpath_ ? 0.1 : 1.0, gragh_[21], gragh_[25]);

    /* add node 26 */
    pos << 301.830, -22.904, 0.01;
    gragh_.push_back(std::make_shared<alpath_graph::GraphNode>(pos, 26));
    /* edge 24 26 */
    createEdge(is_use_alpath_ ? 0.1 : 1.0, gragh_[24], gragh_[26]);

    /* add node 27 */
    pos << 302.335, 12.630, 0.01;
    gragh_.push_back(std::make_shared<alpath_graph::GraphNode>(pos, 27));
    /* edge 26 27 */
    createEdge(is_use_alpath_ ? 0.1 : 1.0, gragh_[26], gragh_[27]);
}

void AL_Path::createEdge(double alpha, alpath_graph::GraphNode::Ptr &node1, alpath_graph::GraphNode::Ptr &node2) {
    alpath_graph::Edge::Ptr edge = std::make_shared<alpath_graph::Edge>(alpha, node1, node2);
//    std::cout << "edge_" << node1->id_ << "_" << node2->id_ << ":"
//    << edge->getWight() << std::endl;

    node1->addNeighbor(node2);
    node1->edges_.push_back(edge);
    node2->addNeighbor(node1);
    node2->edges_.push_back(edge);

}

vector<Eigen::Vector3d> AL_Path::findPath(Eigen::Vector3d &start, Eigen::Vector3d &end) {
    start_ = start;
    end_   = end;
    vector<Eigen::Vector3d> path;
    vector<int> path_id;

    resetGraph();

    //检查起始点与目标点对应的节点
    std::cout << "[active location path]: start id is " << start_id_
    << ", end id is " << end_id_ << std::endl;
    if (start_id_ == end_id_) {
        path.push_back(start_);
        path.push_back(end_);

        raw_path_.clear();
        raw_path_.push_back(start_);
        raw_path_.push_back(gragh_[start_id_]->pos_);
        raw_path_.push_back(end_);
        return path;
    }

    //path search
    vector<alpath_graph::GraphNode::Ptr> open_vec;
    auto& start_node = gragh_[start_id_];
    start_node->state_ = alpath_graph::GraphNode::OPEN;
    start_node->location_quality_ = 0.0;
    open_vec.push_back(start_node);

    while (!open_vec.empty()){
//        std::cout << "[active path] check open list : ";
//        for(auto& node : open_vec){
//            std::cout << node->id_ << " ";
//        } std::cout << std::endl;

        auto& current   = open_vec.back();
        current->state_ = alpath_graph::GraphNode::CLOSE;
        open_vec.pop_back();

        if(current->id_ == end_id_) break;

        searchNeighbors(current, open_vec);
    }

    //回溯路径
    trackBackPath(path, path_id);

    return path;
}

void AL_Path::resetGraph() {
    start_id_ = -1;
    end_id_   = -1;
    double d_s_min = DBL_MAX;
    double d_e_min = DBL_MAX;

    for(auto& node : gragh_){
        //重置所有节点的状态
        node->state_  = alpath_graph::GraphNode::FAR;
        node->parent_ = nullptr;
        node->location_quality_ = 0.0;

        //寻找与起始点、终点距离最近的节点
        double d2_start = (node->pos_ - start_).squaredNorm();
        double d2_end   = (node->pos_ - end_).squaredNorm();

        if(d2_start < d_s_min){
            start_id_ = node->id_;
            d_s_min   = d2_start;
        }

        if(d2_end < d_e_min){
            end_id_ = node->id_;
            d_e_min = d2_end;
        }
    }
}

void AL_Path::searchNeighbors(alpath_graph::GraphNode::Ptr current, vector<alpath_graph::GraphNode::Ptr> &open_vec) {

    for(int i = 0; i < (int)current->neighbors_.size(); i++){
        auto& node = current->neighbors_[i];
        if(node->state_ == alpath_graph::GraphNode::CLOSE)
            continue;

        auto& edge = current->edges_[i];

        int id1 = edge->getNodeID1();
        int id2 = edge->getNodeID2();
        double wtmp = -1;

        /*一般来说 相邻节点的顺序和与其相连的边的顺序一一对应，但不排除出现打乱的情况*/
        if((id1 == current->id_ && id2 == node->id_)
        || (id1 == node->id_ && id2 == current->id_)) {
            wtmp = current->location_quality_ + edge->getWight();
        }
        else{
            for(auto& e : current->edges_){
                id1 = e->getNodeID1();
                id2 = e->getNodeID2();

                if((id1 == current->id_ && id2 == node->id_)
                   || (id1 == node->id_ && id2 == current->id_)){
                    wtmp = current->location_quality_ + e->getWight();
                    break;
                }
            }

            std::cout << "[active location]: search neighbors error !" << std::endl;
            exit(-1);
        }
        /*一般来说 相邻节点的顺序和与其相连的边的顺序一一对应，但不排除出现打乱的情况*/

        //定位质量状态不小于0
        //wtmp = std::max(0.0, wtmp);
        if(node->state_ == alpath_graph::GraphNode::OPEN){
            if(wtmp < node->location_quality_){
                node->location_quality_ = wtmp;
                node->parent_ = current;
            }
        }
        else{
            node->location_quality_ = wtmp;
            node->parent_ = current;
            node->state_  = alpath_graph::GraphNode::OPEN;
            open_vec.push_back(node);

        }

    }

    /* sort open list */
    auto cmp = [](alpath_graph::GraphNode::Ptr node1, alpath_graph::GraphNode::Ptr node2){
        return node1->location_quality_ > node2->location_quality_;
    };

    sort(open_vec.begin(), open_vec.end(), cmp);
}

void AL_Path::trackBackPath(vector<Eigen::Vector3d> &path, vector<int>& path_id) {
    //raw_path_ 用于平板上路径显示，因此包含所有关键路径点
    raw_path_.clear();

    if(gragh_[end_id_]->state_ == alpath_graph::GraphNode::CLOSE){
        int current_id = end_id_;

        path.push_back(end_);
        path_id.push_back(current_id);
        raw_path_.push_back(end_);
        raw_path_.push_back(gragh_[end_id_]->pos_);

        while(current_id != start_id_){
            current_id = gragh_[current_id]->parent_->id_;
            path.push_back(gragh_[current_id]->pos_);
            path_id.push_back(gragh_[current_id]->id_);
            raw_path_.push_back(gragh_[current_id]->pos_);
        }

        path.pop_back();
        path.push_back(start_);
        raw_path_.push_back(start_);
    } else {
        std::cout << "[active location path]: find path fail !" << std::endl;
        return;
    }

    std::reverse(path.begin(), path.end());
    std::reverse(path_id.begin(), path_id.end());
    std::reverse(raw_path_.begin(), raw_path_.end());
     {
        std::cout << "active location path's id : ";
        for(auto& id : path_id)
            std::cout << id << " ";
        std::cout << std::endl;
    }
}
